#include "step_motor.h"

void stepMotor_init(void)
{
}

void stepMotor_power(uint8_t addr, uint8_t state)
{
	while(1){
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_En_Control(addr, state, 0);
		if(addr == STEP_MOTOR_BUFFER[0] && 0xf3 == STEP_MOTOR_BUFFER[1] && 0x02 == STEP_MOTOR_BUFFER[2] && 0x6b == STEP_MOTOR_BUFFER[3]){
		    STEP_MOTOR_BUFFER[3] = 0;
		    break;
		}
	}
}

void stepMotor_clearZero(uint8_t addr)
{
	while(1){
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_Reset_CurPos_To_Zero(addr);
		if(addr == STEP_MOTOR_BUFFER[0]&& 0x0a == STEP_MOTOR_BUFFER[1] && 0x02 == STEP_MOTOR_BUFFER[2] && 0x6b == STEP_MOTOR_BUFFER[3]){
		    STEP_MOTOR_BUFFER[3] = 0;
		    break;
		}
	}
}

void stepMotor_setSpeed(uint8_t addr,uint8_t dir, uint16_t val)
{
	while(1){
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_Vel_Control(addr,dir,val,STEP_MOTOR_ACCELERATED,0);
		if(addr == STEP_MOTOR_BUFFER[0]&& 0xf6 == STEP_MOTOR_BUFFER[1] && 0x02 == STEP_MOTOR_BUFFER[2] && 0x6b == STEP_MOTOR_BUFFER[3]){
		    STEP_MOTOR_BUFFER[3] = 0;
		    break;
		}
	}
}

void stepMotor_setAngle_time(uint8_t addr, float angle, uint16_t tim)
{
	uint8_t dir = 0;
	uint16_t speed = 0;
	uint32_t clk = 0;
	dir = angle >= 0.0f ? 0 : 1;
	clk = (uint32_t)((fabs(angle) * STEP_MOTOR_STEPNUM * STEP_MOTOR_SUBDIVISION) / 360.000f);
	speed = (int)((float)clk /(STEP_MOTOR_STEPNUM * STEP_MOTOR_SUBDIVISION) / ((float)tim / (float)60000));
	while(1){
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_Pos_Control(addr, dir, speed, STEP_MOTOR_ACCELERATED, clk, 0, 0);
		if(addr == STEP_MOTOR_BUFFER[0]&& 0xfd == STEP_MOTOR_BUFFER[1] && 0x02 == STEP_MOTOR_BUFFER[2] && 0x6b == STEP_MOTOR_BUFFER[3]){
		    STEP_MOTOR_BUFFER[3] = 0;
		    break;
		}
	}
}

void stepMotor_setAngle(uint8_t addr, float angle, uint16_t speed)
{
	uint8_t dir = 0;
	uint32_t clk = 0;
	dir = angle >= 0.0f ? 0 : 1;
	clk = (uint32_t)((fabs(angle) * STEP_MOTOR_STEPNUM * STEP_MOTOR_SUBDIVISION) / 360.000f);
	while(1){
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_Pos_Control(addr, dir, speed, STEP_MOTOR_ACCELERATED, clk, 0, 0);
		if(addr == STEP_MOTOR_BUFFER[0]&& 0xfd == STEP_MOTOR_BUFFER[1] && 0x02 == STEP_MOTOR_BUFFER[2] && 0x6b == STEP_MOTOR_BUFFER[3]){
		    STEP_MOTOR_BUFFER[3] = 0;
		    break;
		}
	}
}

void stepMotor_circle_time(uint8_t addr, float angle, uint16_t tim)
{
	float now_angle = stepMotor_getAngle(addr);
	float run_angle = angle - now_angle;
//	DBG_PRINT("now_angle:%f\r\n",now_angle);
//	DBG_PRINT("run_angle:%f\r\n",run_angle);
	stepMotor_setAngle_time(addr, run_angle, tim);
}

void stepMotor_circle(uint8_t addr, float angle, uint16_t speed)
{
	float now_angle = stepMotor_getAngle(addr);
	float run_angle = angle - now_angle;
//	DBG_PRINT("now_angle:%f\r\n",now_angle);
//	DBG_PRINT("run_angle:%f\r\n",run_angle);
	stepMotor_setAngle(addr, run_angle, speed);
}

float map(float input, float in_min, float in_max, float out_min, float out_max) {
    return (input - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

float stepMotor_cmdToAngle(uint8_t *data)
{
	if (0x6b != data[7])
	{
		return 0;
	}
	data += 2;
	int32_t value = ((int32_t)data[1] << 24) | ((int32_t)data[2] << 16) | ((int32_t)data[3] << 8) | data[4];
	int32_t sign = (data[0] == 0x01) ? -1 : 1;
	float angle = (float)sign * ((float)value * 360.000f) / 65536.000f;
	return angle;
}

float stepMotor_getAngle(uint8_t addr)
{
	while (1)
	{
		STEP_MOTOR_DELAY(STEP_MOTOR_DELAY_TIME);
		Emm_V5_Read_Sys_Params(addr, S_CPOS);
		if(addr == STEP_MOTOR_BUFFER[0] && 0x36 == STEP_MOTOR_BUFFER[1]){
		    STEP_MOTOR_BUFFER[1] = 0;
		    break;
		}
	}
//	DBG_PRINT("%d\r\n",(int)stepMotor_cmdToAngle(STEP_MOTOR_BUFFER));
	return stepMotor_cmdToAngle((uint8_t*)STEP_MOTOR_BUFFER);
}
